path = []
father = {}
def randomInt(s,e):
    if s>e:
        s,e = e,s
    return int(random(s,e))


sx,sy =  (int(random(0,10)),int(random(0,10)))# (5,4) 
ex,ey =  (int(random(0,10)),int(random(0,10)))# (8,4)
state = [False]

forbids = [(randomInt(sx ,ex ),randomInt(sy ,ey )) for i in range(5)]
forbids = [u for u in forbids if u not in [(sx,sy),(ex,ey )]]
def bfs_finder(startp,endp):   
    print(('start',startp,endp)) 
    has_visited = []
    search_pos = [startp]
    cur = ''
    while search_pos!=[] and cur != endp:
        cur = search_pos.pop()
        has_visited.append(cur)
        dmap = {'left':(-1,0),'right':(1,0),'up':(0,-1),'down':(0,1)}
        for direction, displacement in dmap.items():
            
            dx,dy =displacement
            cx,cy = cur
            if 0<= cx+dx <10 and 0<= cy+dy<10:
                if (cx+dx,cy+dy) not in has_visited:
                    search_pos.append((cx+dx,cy+dy))
      
        if len(search_pos)>10*10:
            break
 
    return has_visited
        
        
def Astar_finder(startp,endp,forbids):
    node_father = {}
    print(('start',startp,endp)) 
    has_visited = []
    search_pos = [startp]
    cur = ''
    assert startp not in forbids
    assert endp not in forbids
    while search_pos!=[] and cur != endp:
        search_pos = sorted(search_pos,key=lambda x : abs(x[0]-endp[0])+abs(x[1]-endp[1]) ,reverse=True)
        cur = search_pos.pop()
        has_visited.append(cur)
        dmap = {'left':(-1,0),'right':(1,0),'up':(0,-1),'down':(0,1)}
        for direction, displacement in dmap.items():
            dx,dy =displacement
            cx,cy = cur
           
            if 0<= cx+dx <10 and 0<= cy+dy<10:
                if (cx+dx,cy+dy) in forbids:
                    continue
                if (cx+dx,cy+dy) not in has_visited:
                    search_pos.append((cx+dx,cy+dy))
                    node_father[(cx+dx,cy+dy)] = cur
        
        if len(search_pos)>10*10:
            break
    
    if  len(has_visited) != len(set(has_visited)):
        print('!!no eq')
    return has_visited,node_father
        
def get_path(node_father,end_node):
    
    cur = end_node
    paths = [cur]
    while cur in node_father:
        cur = node_father[cur]
        paths.insert(0,cur)
    return paths
    
def simple_finder(startp,endp):
    x1,y1 = startp
    current_pos = PVector(x1,y1)
    whole_path = [startp]
    x2,y2 = endp
    if (current_pos.x,current_pos.y) == endp:
        return whole_path
    else:
        while (current_pos.x,current_pos.y) != endp:
            dir = ''
            if current_pos.x!= endp[0]:
                if current_pos.x > endp[0]:
                    dir = 'left'
                else:
                    dir = 'right'
            elif current_pos.y!= endp[1]:
                if current_pos.y > endp[1]:
                    dir = 'up'
                else:
                    dir = 'down'
            dmap = {'left':(-1,0),'right':(1,0),'up':(0,-1),'down':(0,1)}
            dx,dy = dmap[dir]
            current_pos.add(PVector(dx,dy ))
            whole_path.append((current_pos.x,current_pos.y))
    return whole_path
        
def setup():
    size(800,600)
  
    #path = simple_finder((5,4),(2,2))
    path_,father_ = Astar_finder((sx,sy),(ex,ey),forbids)
    path.extend(path_)    
    father.update(father_)
def draw():
    
    if not  state[0]:
        background(255)
        for row in range(10):
            for col in range(10):
                stroke(0)
                fill(255)
                rect( col*40,row*40,40,40)
        
        print(path)
        i = 0
        for x,y in path:
            # 中间色
            fill(205, 193, 255)
            rect( x*40,y*40,40,40)
            fill(0)
           
            i +=1
        # 起点浅色
        fill(229, 217, 242)
        rect( sx*40,sy*40,40,40)
        fill(0)
        text('S',sx*40+20,sy*40+40 )
        # 终点深色
        fill(165, 148, 249)
        rect( ex*40,ey*40,40,40)
        fill(0)
        text('E',ex*40+20,ey*40+40 )
        for px,py in forbids:
            fill(50)
            rect( px*40,py*40,40,40)
        
        i = 0
        for x,y in path:    
            fill(0)
            text(str(i),x*40,y*40+20)
            i +=1
            
        real = get_path(father,(ex,ey))
        for px,py in real:    
            stroke(255,0,0)
            noFill()
            rect( px*40,py*40,40,40)
    state[0] = True
    